/*
 * 该代码是为三种landmark用cartographer实现
 * ---此节点需要与cartographer和DM-reader--tcp_node协同工作
 * ---确保cartographer发布话题: /tracked_pose -- 并在cartographer配置文件中打开
 */

#include <rclcpp/rclcpp.hpp>              // 【ROS2】包含rclcpp头文件替换ros/ros.h
#include <sensor_msgs/msg/laser_scan.hpp> // 【ROS2】消息头修改
#include <visualization_msgs/msg/marker_array.hpp>
#include <cartographer_ros_msgs/msg/landmark_list.hpp>
#include <cartographer_ros_msgs/msg/landmark_entry.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <Eigen/Dense>
#include <unordered_map>
#include <vector>
#include <cmath>
#include <queue>
//  #include <tf/LinearMath/Quaternion.h>      // 【ROS2】tf头文件修改（推荐使用tf2）
#include <regex>
#include <fstream>
#include <iostream>

// ROS2中Service头文件一般与服务接口文件相对应
#include "param_space/srv/get_param.hpp"
#include "param_space/msg/param_changes.hpp" // 【ROS2】消息换成msg类型
#include "param_space/srv/set_param.hpp"

// 参数
bool use_reflect_panel;
bool use_reflect_column;
double intensity_threshold;
int min_colum_points_per_cluster;
int min_panel_points_per_cluster;
double max_circle_fit_error;
double max_line_fit_error;
double column_radius;
double position_threshold;
double panel_length;
double panel_length_noise;
double translation_weight;
std::string frame_id;

// 全局变量（建议封装在类中，此处为了与原版结构相似）
std::unordered_map<std::string, geometry_msgs::msg::Pose> landmark_cricle_pose_map;
std::unordered_map<std::string, geometry_msgs::msg::Pose> landmark_line_pose_map;
int current_landmark_id = 0;
size_t continue_count = 0;
size_t line_count = 0;
size_t circle_count = 0;
bool success_relocated = false;
struct intensity_ponit
{
    double x;
    double y;
    double intensity;
};

class Fit
{
public:
    double line_fit_error;
    Eigen::Vector2d line_center;
    double cricle_fit_error;
    Eigen::Vector2d circle_center;
    double radius;
    double max_lenth;
    Fit() {}
    void fit_line(const std::vector<intensity_ponit> &points);
    void fit_circle(const std::vector<intensity_ponit> &points);
    void lenth(const std::vector<intensity_ponit> &points);
};

void Fit::fit_line(const std::vector<intensity_ponit> &points)
{
    double m, b, sum_x, sum_y;
    int n = points.size();
    Eigen::MatrixXd A(n, 2);
    Eigen::VectorXd B(n);
    for (int i = 0; i < n; ++i)
    {
        A(i, 0) = points[i].x; // x
        A(i, 1) = 1.0;         // 截距项 b
        B(i) = points[i].y;    // y
    }
    Eigen::Vector2d solution = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);
    m = solution(0); // 直线斜率
    b = solution(1); // 截距
    double mse = 0.0;
    sum_x = 0.0;
    sum_y = 0.0;
    for (const auto &point : points)
    {
        sum_x += point.x;
        sum_y += point.y;
        double predicted_y = m * point.x + b;
        mse += std::fabs(predicted_y - point.y);
    }
    double mean_x = sum_x / n;
    double mean_y = sum_y / n;
    line_fit_error = (mse / n) * 100;
    line_center.x() = mean_x;
    line_center.y() = mean_y;
    // std::cout << "line_center:" << line_center.x() << "," << line_center.y() << "line_fit_error:" << line_fit_error << "\n"
    //   << std::endl;
}

void Fit::fit_circle(const std::vector<intensity_ponit> &points)
{
    int n = points.size();
    Eigen::MatrixXd A(n, 3);
    Eigen::VectorXd B(n);

    for (int i = 0; i < n; ++i)
    {
        double x = points[i].x;
        double y = points[i].y;
        A(i, 0) = 2 * x;
        A(i, 1) = 2 * y;
        A(i, 2) = 1;
        B(i) = (x * x + y * y);
    }
    Eigen::Vector3d X = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);
    double cx = X(0);
    double cy = X(1);
    double c = X(2);
    circle_center.x() = cx;
    circle_center.y() = cy;
    radius = (sqrt(cx * cx + cy * cy + c)) * 100;
    // 计算圆拟合误差
    double MSE = 0.0;
    for (const auto &point : points)
    {
        double dx = point.x - circle_center.x();
        double dy = point.y - circle_center.y();
        double distance = (sqrt(dx * dx + dy * dy)) * 100;
        MSE += std::fabs(distance - radius);
    }
    cricle_fit_error = (MSE / n);
    // std::cout << "circle_center:" << circle_center.x() << "," << circle_center.y() << "radius:"
    //           << radius << "cricle_fit_error:" << cricle_fit_error << "\n"
    //           << std::endl;
}

void Fit::lenth(const std::vector<intensity_ponit> &cluster)
{
    max_lenth = 0;
    for (size_t i = 0; i < cluster.size(); i++)
    {
        for (size_t j = 0; j < cluster.size(); j++)
        {
            double dx = cluster.at(i).x - cluster.at(j).x;
            double dy = cluster.at(i).y - cluster.at(j).y;
            double lenth = sqrt(dx * dx + dy * dy);
            if (lenth > max_lenth)
            {
                max_lenth = lenth;
            }
        }
    }
}
// 提取强度高于阈值的点
std::vector<intensity_ponit> withdraw_high_intensity_points(const sensor_msgs::msg::LaserScan::SharedPtr scan)
{
    std::vector<intensity_ponit> intens_points;
    intensity_ponit point;
    for (size_t i = 0; i < scan->ranges.size(); ++i)
    {
        if (scan->intensities[i] > intensity_threshold)
        {
            double angle = scan->angle_min + i * scan->angle_increment;
            point.x = scan->ranges[i] * cos(angle);
            point.y = scan->ranges[i] * sin(angle);
            point.intensity = scan->intensities[i];
            intens_points.push_back(point);
        }
    }
    return intens_points;
}

std::vector<std::vector<intensity_ponit>> Cluster_points(const std::vector<intensity_ponit> &points)
{
        std::vector<std::vector<intensity_ponit>> clusters;
    if (points.empty())
    {
        return clusters;
    }

    std::vector<bool> visited(points.size(), false);
    for (size_t i = 0; i < points.size(); ++i)
    {
        if (!visited[i])
        {
            std::vector<intensity_ponit> cluster;
            std::queue<size_t> q;
            q.push(i);
            visited[i] = true;

            while (!q.empty())
            {
                size_t current = q.front();
                q.pop(); // Removes first element.
                cluster.push_back(points[current]);

                // 遍历所有点，寻找未访问的邻近点
                for (size_t j = 0; j < points.size(); ++j)
                {
                    if (!visited[j])
                    {
                        Eigen::Vector2d p1(points[current].x, points[current].y);
                        Eigen::Vector2d p2(points[j].x, points[j].y);
                        if ((p1 - p2).norm() < 0.03)
                        {
                            q.push(j);
                            visited[j] = true;
                        }
                    }
                }
            }
            clusters.push_back(cluster);
        }
    }
    return clusters;
}

// 计算landmarks全局坐标（此函数未调用ROS接口，保持基本不变）
geometry_msgs::msg::Pose calculateWorldPose(const geometry_msgs::msg::Pose &lidar_landmark_pose, const geometry_msgs::msg::PoseStamped &robot_gl_pose)
{
    geometry_msgs::msg::Pose world_pose;

    double rx = robot_gl_pose.pose.position.x;
    double ry = robot_gl_pose.pose.position.y;
    const geometry_msgs::msg::Pose &q = robot_gl_pose.pose;
    // 【ROS2】建议使用tf2库来计算欧拉角，此处保留简化版本
    double theta = atan2(2.0 * (q.orientation.w * q.orientation.z + q.orientation.x * q.orientation.y),
                         1.0 - 2.0 * (q.orientation.y * q.orientation.y + q.orientation.z * q.orientation.z));
    double lx = lidar_landmark_pose.position.x;
    double ly = lidar_landmark_pose.position.y;

    world_pose.position.x = rx + lx * cos(theta) - ly * sin(theta);
    world_pose.position.y = ry + lx * sin(theta) + ly * cos(theta);
    world_pose.position.z = 0.0;

    world_pose.orientation.w = 1.0;
    world_pose.orientation.x = 0.0;
    world_pose.orientation.y = 0.0;
    world_pose.orientation.z = 0.0;

    return world_pose;
}

// 计算聚类内点的平均强度
double mean_intensity(const std::vector<intensity_ponit> &cluster)
{
    double mean = 0;
    for (size_t i = 0; i < cluster.size(); i++)
    {
        mean += cluster.at(i).intensity;
    }
    return mean / cluster.size();
}

// 封装所有回调和逻辑在一个类中 【ROS2】节点类
class LandmarkPublisher : public rclcpp::Node
{
public:
    LandmarkPublisher() : Node("landmark_publisher")
    {
        // 参数声明和获取 【ROS2】
        this->declare_parameter<bool>("use_reflect_panel", true);
        this->declare_parameter<bool>("use_reflect_column", true);
        this->declare_parameter<double>("intensity_threshold", 800.0);
        this->declare_parameter<int>("min_colum_points_per_cluster", 5);
        // 此处为了示例，两个min_points_per_cluster各取不同参数，可根据需要分开声明
        this->declare_parameter<int>("min_panel_points_per_cluster", 10);
        this->declare_parameter<double>("max_circle_fit_error", 0.15);
        this->declare_parameter<double>("max_line_fit_error", 5.0);
        this->declare_parameter<double>("column_radius", 5.0);
        this->declare_parameter<double>("position_threshold", 1.0);
        this->declare_parameter<double>("panel_length", 0.2);
        this->declare_parameter<double>("panel_length_noise", 0.05);
        this->declare_parameter<double>("translation_weight", 50.0);
        this->declare_parameter<std::string>("frame_id;","base_link");

        use_reflect_panel = this->get_parameter("use_reflect_panel").as_bool();
        use_reflect_column = this->get_parameter("use_reflect_column").as_bool();
        intensity_threshold = this->get_parameter("intensity_threshold").as_double();
        min_colum_points_per_cluster = this->get_parameter("min_colum_points_per_cluster").as_int();
        min_panel_points_per_cluster = this->get_parameter("min_panel_points_per_cluster").as_int();
        max_circle_fit_error = this->get_parameter("max_circle_fit_error").as_double();
        max_line_fit_error = this->get_parameter("max_line_fit_error").as_double();
        column_radius = this->get_parameter("column_radius").as_double();
        position_threshold = this->get_parameter("position_threshold").as_double();
        panel_length = this->get_parameter("panel_length").as_double();
        panel_length_noise = this->get_parameter("panel_length_noise").as_double();
        translation_weight = this->get_parameter("translation_weight").as_double();
        frame_id = this->get_parameter("frame_id;").as_string();
        // 创建发布器 【ROS2】
        landmark_pub_ = this->create_publisher<cartographer_ros_msgs::msg::LandmarkList>("landmark", 10);

        // 创建订阅器 【ROS2】

        dm_sub_ = this->create_subscription<cartographer_ros_msgs::msg::LandmarkList>(
            "DM_pose", 10,
            std::bind(&LandmarkPublisher::DMcallback, this, std::placeholders::_1));

        // 订阅参数变化 【ROS2】建议使用Timer或参数回调（此处简单用订阅模拟）
        param_changes_sub_ = this->create_subscription<param_space::msg::ParamChanges>(
            "param_changes", 10,
            std::bind(&LandmarkPublisher::param_changes_handler, this, std::placeholders::_1));

        // 创建服务客户端 【ROS2】
        client_get_param_ = this->create_client<param_space::srv::GetParam>("get_param");

        RCLCPP_INFO(this->get_logger(), "LandmarkPublisher node started.");
    }

private:
    // 记录机器人全局位姿 【ROS2】使用PoseStamped保存位姿信息
    geometry_msgs::msg::PoseStamped robot_gl_pose_;
    std_msgs::msg::Header scan_header_;

    // 订阅器与发布器
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr traced_pose_sub_;
    rclcpp::Subscription<visualization_msgs::msg::MarkerArray>::SharedPtr landmark_pose_list_sub_;
    rclcpp::Subscription<cartographer_ros_msgs::msg::LandmarkList>::SharedPtr dm_sub_;
    rclcpp::Subscription<param_space::msg::ParamChanges>::SharedPtr param_changes_sub_;
    rclcpp::Publisher<cartographer_ros_msgs::msg::LandmarkList>::SharedPtr landmark_pub_;
    rclcpp::Client<param_space::srv::GetParam>::SharedPtr client_get_param_;

    bool flag_started_{false};
    bool slam_mode_{true};

    // 回调函数【ROS2】回调函数参数由ROS1改为SharedPtr类型

    void scanCallback(const sensor_msgs::msg::LaserScan::SharedPtr scan)
    {
        // std::cout << "get_scan" << std::endl;
        Fit fit;
        bool maybe_landmark = false;
        cartographer_ros_msgs::msg::LandmarkList landmark_list_msg;
        cartographer_ros_msgs::msg::LandmarkEntry landmark_circle;
        cartographer_ros_msgs::msg::LandmarkEntry landmark_line;
        scan_header_ = scan->header;
        landmark_list_msg.header.stamp = scan->header.stamp;
        landmark_list_msg.header.frame_id = scan->header.frame_id;
        // 提取高强度点
        auto high_intens_points = withdraw_high_intensity_points(scan);
        // 聚类
        auto clusters_points = Cluster_points(high_intens_points);
        // std::cout << "clusters_size" << clusters_points.size() << std::endl;
        for (const auto &cluster : clusters_points)
        {
            // std::cout << "cluster_size" << cluster.size() << std::endl;
            if (cluster.size() > min_colum_points_per_cluster * 0.8)
            {
                maybe_landmark = true;
                bool is_circle = false;
                bool is_line = false;
                double mean_intens = mean_intensity(cluster);
                // std::cout << "mean_intensity:" << mean_intens << "cluster_size:" << cluster.size() << "\n"
                //           << std::endl;
                fit.fit_circle(cluster);
                fit.fit_line(cluster);
                fit.lenth(cluster);
                // std::cout << "max_lenth:" << fit.max_lenth << std::endl;
                if (use_reflect_column)
                {
                    if (fit.cricle_fit_error < max_circle_fit_error && fit.radius < (column_radius + 2.0) &&
                        (column_radius - 1.0) < fit.radius &&
                        ((2 * column_radius - 2) * 0.7) < (fit.max_lenth * 100) &&
                        ((2 * column_radius + 4) * 0.9) > (fit.max_lenth * 100))
                    {
                        circle_count++;
                        is_circle = true;
                        // RCLCPP_INFO(this->get_logger(), "maybe反光柱");
                        // std::cout<<"maybe反光柱"<<std::endl;
                        landmark_circle.tracking_from_landmark_transform.position.x = fit.circle_center.x();
                        landmark_circle.tracking_from_landmark_transform.position.y = fit.circle_center.y();
                        landmark_circle.tracking_from_landmark_transform.position.z = 0.0;
                        landmark_circle.tracking_from_landmark_transform.orientation.w = 1.0;
                        landmark_circle.translation_weight = translation_weight;
                        landmark_circle.rotation_weight = 0;

                        geometry_msgs::msg::Pose lidar_landmark_pose;
                        lidar_landmark_pose.position.x = landmark_circle.tracking_from_landmark_transform.position.x;
                        lidar_landmark_pose.position.y = landmark_circle.tracking_from_landmark_transform.position.y;
                        auto world_landmark_pose = calculateWorldPose(lidar_landmark_pose, robot_gl_pose_);
                        // ID管理
                        if (landmark_cricle_pose_map.empty())
                        {
                            landmark_circle.id = "landmark0" + std::to_string(current_landmark_id);
                        }
                        else
                        {
                            bool found_similar_landmark = false;
                            int max_existing_id = 0;
                            for (const auto &item : landmark_cricle_pose_map)
                            {
                                std::string id = item.first;
                                geometry_msgs::msg::Pose stored_pose = item.second;
                                double dx = stored_pose.position.x - world_landmark_pose.position.x;
                                double dy = stored_pose.position.y - world_landmark_pose.position.y;
                                double dz = stored_pose.position.z - world_landmark_pose.position.z;
                                double distance = sqrt(dx * dx + dy * dy + dz * dz);
                                if (distance < position_threshold)
                                {
                                    landmark_circle.id = id;
                                    found_similar_landmark = true;
                                    break;
                                }
                                int num_id = std::stoi(id.substr(9));
                                if (num_id > max_existing_id)
                                {
                                    max_existing_id = num_id;
                                }
                            }
                            if (!found_similar_landmark)
                            {
                                landmark_circle.id = "landmark0" + std::to_string(max_existing_id + 1);
                            }
                        }
                    }
                }
                if (use_reflect_panel)
                {
                    if (fit.line_fit_error < max_line_fit_error && mean_intens > intensity_threshold  &&
                        cluster.size() > min_panel_points_per_cluster  &&
                        fit.max_lenth < (panel_length + panel_length_noise) && fit.max_lenth > (panel_length - panel_length_noise))
                    {
                        line_count++;
                        is_line = true;
                        // RCLCPP_INFO(this->get_logger(), "maybe反光贴");
                        // std::cout<<"maybe反光tie"<<std::endl;
                        landmark_line.tracking_from_landmark_transform.position.x = fit.line_center.x();
                        landmark_line.tracking_from_landmark_transform.position.y = fit.line_center.y();
                        landmark_line.tracking_from_landmark_transform.position.z = 0.0;
                        landmark_line.tracking_from_landmark_transform.orientation.w = 1.0;
                        landmark_line.translation_weight = translation_weight;
                        landmark_line.rotation_weight = 0;

                        geometry_msgs::msg::Pose lidar_landmark_pose;
                        lidar_landmark_pose.position.x = landmark_line.tracking_from_landmark_transform.position.x;
                        lidar_landmark_pose.position.y = landmark_line.tracking_from_landmark_transform.position.y;
                        auto world_landmark_pose = calculateWorldPose(lidar_landmark_pose, robot_gl_pose_);
                        if (landmark_line_pose_map.empty())
                        {
                            landmark_line.id = "landmark1" + std::to_string(current_landmark_id);
                        }
                        else
                        {
                            bool found_similar_landmark = false;
                            int max_existing_id = 0;
                            for (const auto &item : landmark_line_pose_map)
                            {
                                std::string id = item.first;
                                geometry_msgs::msg::Pose stored_pose = item.second;
                                double dx = stored_pose.position.x - world_landmark_pose.position.x;
                                double dy = stored_pose.position.y - world_landmark_pose.position.y;
                                double dz = stored_pose.position.z - world_landmark_pose.position.z;
                                double distance = sqrt(dx * dx + dy * dy + dz * dz);
                                if (distance < position_threshold)
                                {
                                    landmark_line.id = id;
                                    found_similar_landmark = true;
                                    break;
                                }
                                int num_id = std::stoi(id.substr(9));
                                if (num_id > max_existing_id)
                                {
                                    max_existing_id = num_id;
                                }
                            }
                            if (!found_similar_landmark)
                            {
                                landmark_line.id = "landmark1" + std::to_string(max_existing_id + 1);
                            }
                        }
                    }
                }
                if (!is_circle && !is_line)
                {
                    continue;
                }
                if (is_circle && circle_count > (line_count + 2))
                {
                    std::cout << "pub反光柱" << std::endl;
                    landmark_list_msg.landmarks.push_back(landmark_circle);
                }
                if (is_line && line_count > (circle_count + 2))
                {
                    std::cout << "pub反光贴" << std::endl;
                    landmark_list_msg.landmarks.push_back(landmark_line);
                }
            }
        }
        if (!maybe_landmark)
        {
            continue_count++;
        }
        if (continue_count > 25)
        {
            continue_count = 0;
            circle_count = 0;
            line_count = 0;
        }
        landmark_pub_->publish(landmark_list_msg);
    }

    void tracedPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
    {
        robot_gl_pose_ = *msg;
    }

    void landmarkPoseListCallback(const visualization_msgs::msg::MarkerArray::SharedPtr msg)
    {
        for (const auto &marker : msg->markers)
        {
            std::string text_id = marker.text;
            geometry_msgs::msg::Pose pose = marker.pose;
            if (text_id[8] == '0')
            {
                if (landmark_cricle_pose_map.find(text_id) != landmark_cricle_pose_map.end())
                {
                    landmark_cricle_pose_map[text_id] = pose;
                }
                else
                {
                    landmark_cricle_pose_map.insert({text_id, pose});
                }
            }
            else if (text_id[8] == '1')
            {
                if (landmark_line_pose_map.find(text_id) != landmark_line_pose_map.end())
                {
                    landmark_line_pose_map[text_id] = pose;
                }
                else
                {
                    landmark_line_pose_map.insert({text_id, pose});
                }
            }
            else
            {
                continue;
            }
        }
    }

    void DMcallback(const cartographer_ros_msgs::msg::LandmarkList::SharedPtr qr_pose)
    {
        cartographer_ros_msgs::msg::LandmarkList qr_msg = *qr_pose;
        qr_msg.header.stamp = scan_header_.stamp;
        qr_msg.header.frame_id = frame_id;
        landmark_pub_->publish(qr_msg);
        RCLCPP_WARN(this->get_logger(), "publish_DM_landmark");
    }

    // 参数变化处理回调 【ROS2】建议通过参数回调，本处保留原订阅方式
    void param_changes_handler(const param_space::msg::ParamChanges::SharedPtr changes)
    {
        std::cout << "get_param_changes" << std::endl;
        if (flag_started_)
        {
            return;
        }
        bool is_slam_mode = false;
        bool is_global_relocated = false;
        for (auto &&i : changes->changed_params)
        {
            std::cout << "get_param_changes:" << i << std::endl;
            if (i == "slam_mode")
            {
                RCLCPP_WARN(this->get_logger(), "slam_mode");
                is_slam_mode = true;
            }
            else if (i == "global_relocated")
            {
                RCLCPP_WARN(this->get_logger(), "relocation_mode");
                is_global_relocated = true;
            }
        }
        if (is_slam_mode)
        {
            RCLCPP_INFO(this->get_logger(), "slam_mode == true");

            // 构造请求
            auto request = std::make_shared<param_space::srv::GetParam::Request>();
            request->keys.push_back("slam_mode");

            // 等待服务可用
            if (!client_get_param_->wait_for_service(std::chrono::seconds(2)))
            {
                RCLCPP_WARN(this->get_logger(), "get_param 服务未就绪");
                return;
            }

            // 正确调用 async_send_request(request, callback) 重载
            client_get_param_->async_send_request(
                request,
                // callback 必须签名为：void(SharedFuture)
                [this](rclcpp::Client<param_space::srv::GetParam>::SharedFuture future)
                {
                    // future.get() 就是服务端返回的 response::SharedPtr
                    auto response = future.get();
                    if (response->opcode == 0 && !response->vals.empty())
                    {
                        bool val = (response->vals[0] == "true");
                        slam_mode_ = val;
                        RCLCPP_INFO(this->get_logger(), "slam_mode = %s", val ? "true" : "false");
                        if (val)
                        {
                            std::cout << "slam start" << std::endl;
                            start();
                        }
                    }
                    else
                    {
                        RCLCPP_ERROR(this->get_logger(), "get_param 调用失败 或 返回数据为空");
                    }
                });
        }

        if (is_global_relocated)
        {
            RCLCPP_INFO(this->get_logger(), "Requesting global_relocated...");

            // 构造请求
            auto request = std::make_shared<param_space::srv::GetParam::Request>();
            request->keys.push_back("global_relocated");

            // 等待服务可用
            if (!client_get_param_->wait_for_service(std::chrono::seconds(2)))
            {
                RCLCPP_WARN(this->get_logger(), "get_param 服务未就绪");
                return;
            }

            // 异步发送请求，并用 lambda 回调处理响应
            client_get_param_->async_send_request(
                request,
                [this](rclcpp::Client<param_space::srv::GetParam>::SharedFuture future)
                {
                    auto response = future.get();
                    if (response->opcode == 0 && !response->vals.empty())
                    {
                        bool val = (response->vals[0] == "true");
                        success_relocated = val;
                        RCLCPP_INFO(this->get_logger(),
                                    "global_relocated = %s", val ? "true" : "false");
                        if (val)
                        {
                            std::cout << "located start" << std::endl;
                            start();
                        }
                    }
                    else
                    {
                        RCLCPP_ERROR(this->get_logger(),
                                     "get_param(global_relocated) 调用失败或返回空");
                    }
                });
        }
    }

    // 模拟启动订阅【ROS2】仅在第一次启动时订阅
    void start()
    {
        std::cout << "start" << std::endl;
        if (!flag_started_)
        {
            flag_started_ = true;
            scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
                "scan", 10,
                std::bind(&LandmarkPublisher::scanCallback, this, std::placeholders::_1));
            traced_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
                "tracked_pose", 10,
                std::bind(&LandmarkPublisher::tracedPoseCallback, this, std::placeholders::_1));
            landmark_pose_list_sub_ = this->create_subscription<visualization_msgs::msg::MarkerArray>(
                "landmark_poses_list", 10,
                std::bind(&LandmarkPublisher::landmarkPoseListCallback, this, std::placeholders::_1));
            RCLCPP_INFO(this->get_logger(), "Subscriptions already started.");
        }
    }
};

int main(int argc, char **argv)
{
    // 【ROS2】节点初始化
    rclcpp::init(argc, argv);
    auto node = std::make_shared<LandmarkPublisher>();

    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
